/********************************************
* Copyrigt (C) rumi
* Module: serv_pid.c
*********************************************/
#include "serv_pid.h"
static const char *TAG = "ser_pid.c";


void Serv_PID_Reset(pid_uint_typedef *pPIDContrl)
{
    pPIDContrl->U_kk=0;
	pPIDContrl->ekk=0;
	pPIDContrl->ekkk=0;
}

int16_t Serv_PID_Common(int SetVal,int NowVal,pid_uint_typedef *pPIDContrl)
{
    int ek=0,U_k=0;

	ek = NowVal - SetVal;
	
	U_k=pPIDContrl->U_kk + pPIDContrl->Kp*(ek - pPIDContrl->ekk) + pPIDContrl->Ki*ek + pPIDContrl->Kd*(ek - 2*pPIDContrl->ekk + pPIDContrl->ekkk);
	
	pPIDContrl->U_kk = U_k;
    pPIDContrl->ekkk = pPIDContrl->ekk;
	pPIDContrl->ekk  = ek;
	
	if(U_k > (pPIDContrl->Ur))
		U_k = pPIDContrl->Ur;
	if(U_k < -(pPIDContrl->Ur))
		U_k = -(pPIDContrl->Ur);
	
	return U_k >> 10;
}

void Serv_PID_Control(pid_uint_typedef *pPIDContrl)
{
    if(pPIDContrl->En == 1)
	{
		pPIDContrl->Adjust = Serv_PID_Common(pPIDContrl->ValSet, pPIDContrl->ValNow, pPIDContrl);
	}
	else
	{
		pPIDContrl->Adjust = 0;
		Serv_PID_Reset(pPIDContrl);
		pPIDContrl->En = 2;
	}
}



/********************************************
 * End of file
*********************************************/